#include "C_H.h"
// #include "kalman.h"
//ceshi 测试第二次提交
// 在全局变量区域添加
std::atomic<bool> should_exit{false}; // 程序退出标志

int ding_shi=3000;

// PWM信息结构体
struct pwm_info motor_1_pwm_info;
struct pwm_info motor_2_pwm_info;
struct pwm_info servo_pwm_info;
struct pwm_info pwm_1_info;
struct pwm_info pwm_2_info;

// 另外一端的IP地址
#define SERVER_IP "192.168.1.114"
// 端口号
#define PORT 8086
uint8_t tcp_suc_flag = 0;

// 清理函数
void cleanup()
{
    // 先停止所有PWM输出
    pwm_set_duty(MOTOR1_PWM, 0);
    pwm_set_duty(MOTOR2_PWM, 0);
    pwm_set_duty(SERVO_PWM, 0);

    // 等待一小段时间确保PWM已经停止
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    // 关闭摄像头
    if (cap.isOpened())
    {
        cap.release();
    }

    // int result = system("tar -cvf fitting.tar fitting && tar -cvf origin.tar origin && tar -cvf data.tar data");
    // if (result == -1)
    // {
    //     std::cerr << "Error executing the command!" << std::endl;
    // }

    // 清除显示
    ips200_clear();
    gpio_set_level(BEEP, 0x0);

    std::cout << "清理完成，程序正常退出" << std::endl;
}

// 信号处理函数
void signal_handler(int signum)
{
    std::cout << "\n收到信号 " << signum << "，开始清理..." << std::endl;
    should_exit = true;
    cleanup();
    exit(0);
}

// 系统初始化
void init_system(void)
{
    // 注册信号处理函数
    signal(SIGINT, signal_handler);  // Ctrl+C
    signal(SIGTERM, signal_handler); // 终止信号
    signal(SIGSEGV, signal_handler); // 段错误

    // 注册程序退出时的清理函数
    atexit(cleanup);

    // 初始化IPS200显示屏
    ips200_init("/dev/fb0"); // 根据实际设备路径调整
    ips200_clear();          // 清屏

    
    #ifdef GYRO
        imu_get_dev_info();
    #endif


    if(DEV_IMU963RA == imu_type)//DEV_IMU660RA == imu_type
    {
        printf("IMU DEV IS DEV_IMU963RA\r\n");
    }
    else
    {
        printf("NO FIND IMU DEV\r\n");
        //return -1;
    }

    // 初始化摄像头
    if (!initCamera())
    {
        exit(-1);
    }

    // 获取PWM设备信息
    pwm_get_dev_info(MOTOR1_PWM, &motor_1_pwm_info);
    pwm_get_dev_info(MOTOR2_PWM, &motor_2_pwm_info);
    pwm_get_dev_info(SERVO_PWM, &servo_pwm_info);
    pwm_get_dev_info(PWM_1, &pwm_1_info);
    pwm_get_dev_info(PWM_2, &pwm_2_info);
    

    kalman_init(&R_encoderkal, R_Q, R_R);
    kalman_init(&L_encoderkal, L_Q, L_R);


    pwm_set_duty(MOTOR1_PWM, 0);
    pwm_set_duty(MOTOR2_PWM, 0);
    pwm_set_duty(SERVO_PWM, 0);
}

// 网络初始化
void init_network(void)
{
    // tcp传输数据 初始化
    {
        if (tcp_client_init(SERVER_IP, PORT) == 0)
        {
            printf("tcp_client ok\r\n");
            // 逐飞助手初始化 设置回调函数
            seekfree_assistant_interface_init(tcp_client_send_data, tcp_client_read_data);
            tcp_suc_flag = 1; // 标记为初始化成功
            tcp_flag = 1;
        }
        else
        {
            printf("tcp_client error\r\n");
            tcp_suc_flag = 0; // 标记为初始化失败
            tcp_flag = 0;
        }
    }
}


#ifdef erdui
void initial_parameters(void)
{
    otsuMinThreshold = 110;   // 默认下限
    otsuMaxThreshold = 255; // 默认上限

    servo_base = 5000;//756;//5025//5030;//759
    dj = 30;

    mohu_flag = 0;


    //转向环
    Curve_PID[0]=16.0;
    Curve_PID[1]=9;
    Curve_PID[2]=0;

    
    float ratio = (float)(Curve_PID[0] - Loc_PID[0]) / 4.0f;
    for(int i=0; i <= 6; i++) {
        Kp_UFF[i] = Loc_PID[0] + i * ratio;
    }

    ratio = (float)(Curve_PID[1] - Loc_PID[1]) / 4.0f;
    for(int i=0; i <= 6; i++) {
        Kd_UFF[i] = Loc_PID[1] + i * ratio;
    }


    //速度环
    speed_l[0]=20;//30
    speed_l[1]=0;//0.4;
    speed_l[2]=0;

    speed_l_d[0]=40;
    speed_l_d[1]=0;
    speed_l_d[2]=15;

    speed_l_integralLimit_zhi = 10500;
    speed_l_integralLimit_wan = 9000;//8000;
    speed_l_integralLimit=4000;
    speed_l_outputLimit=5000; //9000;


    //右轮
    speed_r[0]=20;//30
    speed_r[1]=0;//0.4;
    speed_r[2]=0;

    speed_r_d[0]=40;
    speed_r_d[1]=0;
    speed_r_d[2]=10;

    speed_r_integralLimit_zhi = 10500;
    speed_r_integralLimit_wan = 9000;//8000;
    speed_r_integralLimit=4000;
    speed_r_outputLimit=5000; //9000;

    ackerman_k=1.0;
    chasu_flag = 2;
    chasu_k = 1.5;

   
    lz_speed = 200;//850;

    //负压

    fy_wan = 9500;
    fy_zhi = 8500;

    fy_flag = 1;  

    switch_baohu = 1;
    time_lock = 3500;
    distance = 47.15;
}
#endif

int main(int, char **)
{



    init_system(); // 系统初始化


    
    

    #ifdef yidui
        ips200_show_string(30, 30, "yidui");
        printf("yidui\n\r");
        pwm_set_duty(PWM_1, 5000);
        system_delay_ms(500);
    #endif

    #ifdef erdui
        ips200_show_string(30, 30, "erdui");
        printf("erdui\n\r");
        pwm_set_duty(PWM_1, 5000);
        system_delay_ms(500);
    #endif

    //init_network();


    // // adrc
    // // 左轮参数：r=50, h=0.005, bw=30, b_hat=0.3, kp=1.2, kd=0.3
    // ADRC_Init(&ctrl_left, 50, 0.005f, 30, 0.3f, 1.2f, 0.3f);
    
    // // 右轮参数（可根据实际情况微调）
    // ADRC_Init(&ctrl_right, 50, 0.005f, 30, 0.3f, 1.2f, 0.3f);








    pit_ms_init(5, pit_callback);
    my_menu_init();
    init_parameters();
    initial_parameters();
    //printf("run_strat\n");
    pwm_set_duty(SERVO_PWM, 0);
    run_main_loop(); // 主循环













    // while (1)
    // {
    //     control_motors(1000, 1000);
    // }

    // while (1)
    // {
    //     pwm_set_duty(SERVO_PWM, servo_base);
    // }
    
    // while (1)
    // {
    //     char Key_Num = detect_key();  
    //     printf("Key_Num:%d\r\n", Key_Num);
    //     system_delay_ms(100);

    // }

    

    return 0;
}
